#ifndef ZOROS_ROBOT_MANAGER_H
#define ZOROS_ROBOT_MANAGER_H

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/string.hpp"
#include "zoros/msg/planner_properties.hpp"

#include <mutex>
#include <atomic>
#include <thread>
#include "damiao.h"
#include "elog/elog_interface.h"
#include "robot_algorithm_interface.h"

#define CONTROL_FREQ 250
#define JOINT_CONTROL_TOLERANCE 1e-3

enum class CmdType
{
    MOVE_JOINT = 0,
    MOVE_POSE,
    MOVE_LINE,
    MOVE_NULLSPACE,
    MOVE_TRAJ,
    TEACH_MOVE,
    JOINT_SCAN,
    GRIPPER_FORCE,
    MOVE_JOINT_WAYPOINT,
    MOVE_POSE_WAYPOINT,
    MOVE_LINE_WAYPOINT,
    MOVE_NULLSPACE_WAYPOINT
};

static std::map<CmdType, std::string> mapMoveTypeName = {
        {CmdType::MOVE_JOINT, "MOVE_JOINT"},
        {CmdType::MOVE_POSE, "MOVE_POSE"},
        {CmdType::MOVE_LINE, "MOVE_LINE"},
        {CmdType::MOVE_NULLSPACE, "MOVE_NULLSPACE"},
        {CmdType::MOVE_TRAJ, "MOVE_TRAJ"},
        {CmdType::TEACH_MOVE, "TEACH_MOVE"},
        {CmdType::JOINT_SCAN, "JOINT_SCAN"},
        {CmdType::GRIPPER_FORCE, "GRIPPER_FORCE"},
        {CmdType::MOVE_JOINT_WAYPOINT, "MOVE_JOINT_WAYPOINT"},
        {CmdType::MOVE_POSE_WAYPOINT, "MOVE_POSE_WAYPOINT"},
        {CmdType::MOVE_LINE_WAYPOINT, "MOVE_LINE_WAYPOINT"},
        {CmdType::MOVE_NULLSPACE_WAYPOINT, "MOVE_NULLSPACE_WAYPOINT"},
};

struct CmdMsg
{
    std_msgs::msg::Float64MultiArray::SharedPtr float64MultiArray{nullptr};
    std_msgs::msg::Float64::SharedPtr float64{nullptr};
    std_msgs::msg::Int32::SharedPtr int32{nullptr};
    std_msgs::msg::String::SharedPtr str{nullptr};
    zoros::msg::PlannerProperties::SharedPtr plannerProperties{nullptr};
};

class RobotManager : public rclcpp::Node
{
public:
    RobotManager();

    ~RobotManager() override
    {
        {
            pthread_mutex_lock(&zoros_mutex_);
            running_ = false;
            pthread_mutex_unlock(&zoros_mutex_);
        }
        cv_.notify_one();
        if (joint_state_refresh_thread_.joinable())
        {
            joint_state_refresh_thread_.join();
        }
        if (controller_thread_.joinable())
        {
            controller_thread_.join();
        }
    }

    void start();


private:
    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr cmd_pub_;
    sensor_msgs::msg::JointState joint_state_msg_;
    sensor_msgs::msg::JointState cmd_msg_;
    std::thread joint_state_refresh_thread_;
    bool is_simulation_ = true;
    // bool is_simulation_;
    std::shared_ptr<ZORAL::ZoralInterface> robot_algo_;
    std::shared_ptr<damiao::Motor_Control> dm_;
    std::vector<damiao::Motor> motors_;
    damiao::Motor gripper_;
    pthread_mutex_t zoros_mutex_{};
    // rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr move_joint_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr move_pose_subscriber_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr move_traj_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr move_line_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr move_nullspace_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr teach_move_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr joint_scan_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr gripper_position_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr gripper_ff_subscriber_;
    rclcpp::Subscription<zoros::msg::PlannerProperties>::SharedPtr joint_planner_properties_subscriber_;
    rclcpp::Subscription<zoros::msg::PlannerProperties>::SharedPtr cartesian_planner_properties_subscriber_;
    rclcpp::Subscription<zoros::msg::PlannerProperties>::SharedPtr nullspace_planner_properties_subscriber_;

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr move_joint_waypoint_subscriber_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr move_pose_waypoint_subscriber_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr move_line_waypoint_subscriber_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr move_nullspace_waypoint_subscriber_;

    std::vector<double> mit_kp_;
    std::vector<double> mit_kd_;
    std::vector<double> torque_gain_;
    ZORAL::PlannerProperties joint_planner_properties_;
    ZORAL::PlannerProperties cartesian_planner_properties_;
    ZORAL::PlannerProperties nullspace_planner_properties_;
    size_t robot_dof_ = 7;
    
    std::atomic<double> gripper_ff_torque_nm_{0.0};

    bool running_;
    bool teach_mode_ = false;

    std::thread controller_thread_;
    std::mutex mutex_;
    std::condition_variable cv_;
    std::queue<std::pair<CmdType, CmdMsg>> move_msg_queue_;
    std::unordered_map<CmdType, std::function<void(const CmdMsg &msg)>> move_func_map_;

    void control(const double &t);

    void initializeMotors();

    void initializeController();

    void initializePlanner();

    void publishJointStatesLoop();

    void commandMoveJointCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);

    void commandMovePoseCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);

    void commandMoveLineCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);

    void commandMoveNullspaceCallback(const std_msgs::msg::Float64::SharedPtr msg);

    void commandMoveTrajCallback(const std_msgs::msg::String::SharedPtr msg);

    void commandTeachMoveCallback(const std_msgs::msg::Int32::SharedPtr msg);

    void commandJointScanCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);

    void commandGripperPositionCallback(const std_msgs::msg::Float64::SharedPtr msg);

    void commandGripperFFTorqueCallback(const std_msgs::msg::Float64::SharedPtr msg);
    
    void propertiesJointPlannerCallback(const zoros::msg::PlannerProperties::SharedPtr msg);

    void propertiesCartesianPlannerCallback(const zoros::msg::PlannerProperties::SharedPtr msg);

    void propertiesNullspacePlannerCallback(const zoros::msg::PlannerProperties::SharedPtr msg);

    void commandMoveJointWaypointCallback(const std_msgs::msg::String::SharedPtr msg);

    void commandMovePoseWaypointCallback(const std_msgs::msg::String::SharedPtr msg);

    void commandMoveLineWaypointCallback(const std_msgs::msg::String::SharedPtr msg);

    void commandMoveNullspaceWaypointCallback(const std_msgs::msg::String::SharedPtr msg);

    void controllerLoop();

    void moveJointFunc(const CmdMsg &msg);

    void moveJointWaypointFunc(const CmdMsg &msg);

    void movePoseFunc(const CmdMsg &msg);

    void movePoseWaypointFunc(const CmdMsg &msg);

    void moveLineFunc(const CmdMsg &msg);

    void moveLineWaypointFunc(const CmdMsg &msg);

    void jointScanFunc(const CmdMsg &msg);

    void moveNullSpaceFunc(const CmdMsg &msg);

    void moveNullSpaceWaypointFunc(const CmdMsg &msg);

    void teachMoveFunc(const CmdMsg &msg);

    void moveTrajFunc(const CmdMsg &msg);
};
#endif /*ZOROS_ROBOT_MANAGER_H*/
